import random
import time
import numpy as np
from datetime import datetime

# 地球半径（米）
EARTH_RADIUS = 6371000

# 设备ID列表
device_ids = ["BUS001", "BUS002", "BUS003", "BUS004", "BUS005"]

# ICM42688传感器参数（保持原厂规格）
gyro_noise = 2.8e-3  # 2.8 mdps/√Hz
accel_noise = 70e-6  # 70 μg/√Hz

# 调整数据范围以符合激烈驾驶特性
accel_range = 2.5  # g（增加加速度范围，更激烈的驾驶）
gyro_range = 1500.0  # dps（增加角速度范围，更激进的转向）

# GPS数据范围（以北京长安街为例）
latitude_range = (39.90, 39.92)
longitude_range = (116.35, 116.38)
altitude_range = (40.0, 60.0)
speed_range = (0.0, 70.0)  # 公交车限速70km/h（已调整）
course_range = (0.0, 360.0)

# 激烈驾驶状态（增加加速度和转向速率）
AGGRESSIVE_STATES = {
    "STOP": {"speed": (0, 5), "acceleration": (-0.1, 0.1), "turning_rate": (-5, 5)},
    "DEPARTING": {"speed": (0, 30), "acceleration": (0.3, 0.8), "turning_rate": (-15, 15)},
    "CRUISING": {"speed": (35, 70), "acceleration": (-0.2, 0.2), "turning_rate": (-12, 12)},
    "SLOWING": {"speed": (10, 50), "acceleration": (-0.8, -0.3), "turning_rate": (-20, 20)},
    "TURNING": {"speed": (15, 40), "acceleration": (-0.3, 0.3), "turning_rate": (-30, 30)},
    "BRAKING": {"speed": (10, 35), "acceleration": (-1.2, -0.8), "turning_rate": (-20, 20)},
}

# 公交车运动状态（基础模式）
VEHICLE_STATES = {
    "STOP": {"speed": (0, 2), "acceleration": (-0.05, 0.05), "turning_rate": (-3, 3)},
    "DEPARTING": {"speed": (0, 20), "acceleration": (0.1, 0.3), "turning_rate": (-10, 10)},
    "CRUISING": {"speed": (25, 50), "acceleration": (-0.1, 0.1), "turning_rate": (-8, 8)},
    "SLOWING": {"speed": (5, 40), "acceleration": (-0.3, -0.1), "turning_rate": (-15, 15)},
    "TURNING": {"speed": (10, 25), "acceleration": (-0.15, 0.15), "turning_rate": (-20, 20)},
    "BRAKING": {"speed": (5, 25), "acceleration": (-0.5, -0.3), "turning_rate": (-15, 15)},
}


class Device:
    """传感器设备类，包含设备ID、IMU数据和GPS数据"""

    def __init__(self, device_id=None, accel_range=accel_range, gyro_range=gyro_range):
        """初始化设备对象"""
        self.device_id = device_id if device_id else random.choice(device_ids)
        self.accel_range = accel_range  # 加速度计量程(g)
        self.gyro_range = gyro_range  # 陀螺仪量程(dps)

        # 初始化传感器数据 - 模拟静止状态下的重力加速度
        self.accel_x = 0.0
        self.accel_y = 0.0
        self.accel_z = 9.8  # 重力加速度约为1g

        self.gyro_x = 0.0
        self.gyro_y = 0.0
        self.gyro_z = 0.0

        # 初始化GPS数据
        self.latitude = random.uniform(latitude_range[0], latitude_range[1])
        self.longitude = random.uniform(longitude_range[0], longitude_range[1])
        self.altitude = random.uniform(altitude_range[0], altitude_range[1])
        self.speed = random.uniform(speed_range[0], speed_range[1])
        self.course = random.uniform(course_range[0], course_range[1])
        self.fix_quality = random.choices([0, 1, 2], weights=[0.02, 0.08, 0.9], k=1)[0]
        self.satellites_visible = random.randint(0, 12) if self.fix_quality > 0 else 0

        # 时间戳
        self.timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]

        # 上一次的传感器数据，用于平滑更新
        self.last_accel_x = self.accel_x
        self.last_accel_y = self.accel_y
        self.last_accel_z = self.accel_z
        self.last_gyro_x = self.gyro_x
        self.last_gyro_y = self.gyro_y
        self.last_gyro_z = self.gyro_z

    def update_sensor_data(self):
        """更新传感器数据，更符合ICM42688的特性"""
        # 模拟物理运动 - 基于上一次数据的平滑变化
        alpha = 0.9  # 相关性系数，使数据变化更平滑

        # 模拟运动状态变化的随机因素
        motion_factor = 0.1  # 运动幅度因子

        # 更新加速度数据
        self.accel_x = alpha * self.last_accel_x + (1 - alpha) * random.uniform(-motion_factor, motion_factor)
        self.accel_y = alpha * self.last_accel_y + (1 - alpha) * random.uniform(-motion_factor, motion_factor)
        self.accel_z = alpha * self.last_accel_z + (1 - alpha) * random.uniform(-motion_factor, motion_factor)

        # 确保加速度数据在量程范围内
        self.accel_x = max(min(self.accel_x, self.accel_range), -self.accel_range)
        self.accel_y = max(min(self.accel_y, self.accel_range), -self.accel_range)
        self.accel_z = max(min(self.accel_z, self.accel_range), -self.accel_range)

        # 更新角速度数据
        self.gyro_x = alpha * self.last_gyro_x + (1 - alpha) * random.uniform(-motion_factor,
                                                                              motion_factor) * self.gyro_range / 10
        self.gyro_y = alpha * self.last_gyro_y + (1 - alpha) * random.uniform(-motion_factor,
                                                                              motion_factor) * self.gyro_range / 10
        self.gyro_z = alpha * self.last_gyro_z + (1 - alpha) * random.uniform(-motion_factor,
                                                                              motion_factor) * self.gyro_range / 10

        # 确保角速度数据在量程范围内
        self.gyro_x = max(min(self.gyro_x, self.gyro_range), -self.gyro_range)
        self.gyro_y = max(min(self.gyro_y, self.gyro_range), -self.gyro_range)
        self.gyro_z = max(min(self.gyro_z, self.gyro_range), -self.gyro_range)

        # 添加ICM42688规格的噪声
        self.accel_x += np.random.normal(0, accel_noise)
        self.accel_y += np.random.normal(0, accel_noise)
        self.accel_z += np.random.normal(0, accel_noise)

        self.gyro_x += np.random.normal(0, gyro_noise)
        self.gyro_y += np.random.normal(0, gyro_noise)
        self.gyro_z += np.random.normal(0, gyro_noise)

        # 模拟异常值 - 1%的概率出现
        if random.random() < 0.01:
            self.accel_x = random.uniform(-self.accel_range, self.accel_range)
            self.accel_y = random.uniform(-self.accel_range, self.accel_range)
            self.accel_z = random.uniform(-self.accel_range, self.accel_range)
            self.gyro_x = random.uniform(-self.gyro_range, self.gyro_range)
            self.gyro_y = random.uniform(-self.gyro_range, self.gyro_range)
            self.gyro_z = random.uniform(-self.gyro_range, self.gyro_range)

        # 更新GPS数据
        alpha_gps = 0.98  # GPS数据变化较慢，使用更高的相关性系数

        self.latitude = alpha_gps * self.latitude + (1 - alpha_gps) * random.uniform(-0.0001, 0.0001)
        self.longitude = alpha_gps * self.longitude + (1 - alpha_gps) * random.uniform(-0.0001, 0.0001)
        self.altitude = alpha_gps * self.altitude + (1 - alpha_gps) * random.uniform(-0.5, 0.5)
        self.speed = alpha_gps * self.speed + (1 - alpha_gps) * random.uniform(-5, 5)
        self.course = alpha_gps * self.course + (1 - alpha_gps) * random.uniform(-10, 10)

        # 添加GPS噪声
        gps_noise_std = 0.00001  # GPS位置噪声标准差(约1米)
        altitude_noise_std = 0.5  # 海拔高度噪声标准差(米)
        speed_noise_std = 0.5  # 速度噪声标准差(km/h)
        course_noise_std = 1.0  # 航向角噪声标准差(度)

        self.latitude += np.random.normal(0, gps_noise_std)
        self.longitude += np.random.normal(0, gps_noise_std)
        self.altitude += np.random.normal(0, altitude_noise_std)
        self.speed = max(0, self.speed + np.random.normal(0, speed_noise_std))  # 速度不能为负
        self.course = (self.course + np.random.normal(0, course_noise_std)) % 360  # 确保航向角在0-360度范围内

        # 更新GPS信号质量
        self.fix_quality = random.choices([0, 1, 2], weights=[0.02, 0.08, 0.9], k=1)[0]
        self.satellites_visible = random.randint(0, 12) if self.fix_quality > 0 else 0

        # 更新时间戳
        self.timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]

        # 保存当前数据为上一次数据
        self.last_accel_x = self.accel_x
        self.last_accel_y = self.accel_y
        self.last_accel_z = self.accel_z
        self.last_gyro_x = self.gyro_x
        self.last_gyro_y = self.gyro_y
        self.last_gyro_z = self.gyro_z

        # 打印调试信息
        print(f"Generated accel_x: {self.accel_x:.6f}, accel_y: {self.accel_y:.6f}, accel_z: {self.accel_z:.6f}")

    def to_dict(self):
        """将设备数据转换为字典格式"""
        return {
            'device_id': self.device_id,
            'accel_x': self.accel_x,
            'accel_y': self.accel_y,
            'accel_z': self.accel_z,
            'gyro_x': self.gyro_x,
            'gyro_y': self.gyro_y,
            'gyro_z': self.gyro_z,
            'latitude': self.latitude,
            'longitude': self.longitude,
            'altitude': self.altitude,
            'speed': self.speed,
            'course': self.course,
            'fix_quality': self.fix_quality,
            'satellites_visible': self.satellites_visible,
            'timestamp': self.timestamp
        }

    def __str__(self):
        """返回设备数据的字符串表示"""
        return f"Device {self.device_id} at {self.timestamp}:\n" + \
            f"  加速度: ({self.accel_x:.4f}, {self.accel_y:.4f}, {self.accel_z:.4f}) g\n" + \
            f"  角速度:  ({self.gyro_x:.4f}, {self.gyro_y:.4f}, {self.gyro_z:.4f}) dps\n" + \
            f"  GPS:   ({self.latitude:.6f}, {self.longitude:.6f}), Alt={self.altitude:.1f}m\n" + \
            f"         Speed={self.speed:.1f}km/h, Course={self.course:.1f}°, " + \
            f"Fix={self.fix_quality}, Sats={self.satellites_visible}"


class BusDevice(Device):
    """公交车传感器设备类，深度适配公交运动特性"""

    def __init__(self, device_id=None, accel_range=accel_range, gyro_range=gyro_range, aggressive=False):
        """初始化公交车设备对象"""
        Device.__init__(self, device_id, accel_range, gyro_range)

        # 公交车专属参数
        self.stop_station_prob = 0.3  # 每周期到站概率
        self.station_ids = [f"STATION{i:03d}" for i in range(1, 11)]  # 10个站点
        self.current_station = random.choice(self.station_ids)
        self.next_station = random.choice([s for s in self.station_ids if s != self.current_station])

        # 激烈驾驶模式 - 在调用_get_state_duration()之前定义
        self.aggressive = aggressive
        self.state_params = AGGRESSIVE_STATES if aggressive else VEHICLE_STATES

        # 运动状态管理
        self.state = "STOP" if random.random() < 0.5 else "CRUISING"
        self.time_in_state = 0
        self.state_duration = self._get_state_duration()

        # 公交车物理参数（更符合公交特性）
        self.acceleration = 0.0  # m/s^2
        self.angular_velocity = 0.0  # 转向角速度 (度/秒)
        self.last_station_time = datetime.now()

        # 添加历史记录属性
        self.speed_history = [self.speed] * 100
        self.course_history = [self.course] * 100

        # 打印初始化信息
        driving_mode = "激烈驾驶模式" if aggressive else "正常驾驶模式"
        print(f"公交车 {self.device_id} 初始化 - 当前站点: {self.current_station}, 驾驶模式: {driving_mode}")

    def _get_state_duration(self):
        """根据状态返回合理的持续时间（秒）"""
        if self.state == "STOP":
            # 激烈驾驶模式下站点停留时间更短
            return random.uniform(5, 15) if self.aggressive else random.uniform(10, 30)
        elif self.state in ["DEPARTING", "BRAKING"]:
            return random.uniform(3, 8) if self.aggressive else random.uniform(5, 10)
        elif self.state in ["SLOWING", "TURNING"]:
            return random.uniform(5, 10) if self.aggressive else random.uniform(8, 15)
        else:  # CRUISING
            return random.uniform(15, 30) if self.aggressive else random.uniform(20, 40)

    def update_state(self):
        """更新公交车运动状态（强化公交站点逻辑）"""
        self.time_in_state += 0.01  # 10ms更新一次

        # 到站逻辑（在巡航状态下有概率触发到站）
        if self.state == "CRUISING" and random.random() < self.stop_station_prob:
            self.state = "SLOWING"
            self.time_in_state = 0
            self.next_station = random.choice([s for s in self.station_ids if s != self.current_station])
            print(f"公交车 {self.device_id} 即将到达: {self.next_station}")
            return

        # 状态持续时间判断
        if self.time_in_state >= self.state_duration:
            # 状态转换逻辑（更符合公交运营规律）
            if self.state == "STOP":
                self.state = "DEPARTING"
            elif self.state == "DEPARTING":
                self.state = "CRUISING"
            elif self.state == "CRUISING":
                # 激烈驾驶模式下更频繁地加速减速
                if self.aggressive and random.random() < 0.4:
                    self.state = random.choice(["DEPARTING", "BRAKING", "TURNING"])
                else:
                    self.state = random.choice(["CRUISING", "TURNING", "SLOWING"])
            elif self.state == "SLOWING":
                self.state = "STOP"
                self.current_station = self.next_station
                print(f"公交车 {self.device_id} 到达站点: {self.current_station}")
            elif self.state == "TURNING":
                self.state = "CRUISING"
            elif self.state == "BRAKING":
                self.state = "STOP"

            self.time_in_state = 0
            self.state_duration = self._get_state_duration()
            print(f"公交车 {self.device_id} 状态变更: {self.state}")

    def update_vehicle_physics(self):
        """根据公交车状态更新物理参数（更激烈的驾驶特性）"""
        # 根据驾驶模式选择不同的状态参数
        state_params = AGGRESSIVE_STATES if self.aggressive else VEHICLE_STATES

        # 目标速度与加速度计算
        target_speed = random.uniform(state_params[self.state]["speed"][0], state_params[self.state]["speed"][1])
        self.acceleration = random.uniform(state_params[self.state]["acceleration"][0],
                                           state_params[self.state]["acceleration"][1])

        # 激烈驾驶模式下加速度变化更快
        alpha = 0.7 if self.aggressive else 0.9
        speed_diff = target_speed - self.speed
        self.speed += speed_diff * (0.2 if self.aggressive else 0.1) + self.acceleration * 0.01  # 10ms时间步长

        # 速度限制
        self.speed = max(0, min(self.speed, 70))  # 公交车限速70km/h

        # 转向角速度更新（更激进的转向）
        self.angular_velocity = random.uniform(state_params[self.state]["turning_rate"][0],
                                               state_params[self.state]["turning_rate"][1])

        # 激烈驾驶模式下转向变化更快
        if self.aggressive and abs(self.angular_velocity) > 10:
            self.angular_velocity *= 1.5

        # 航向角更新
        self.course += self.angular_velocity * 0.01  # 10ms转向增量
        self.course = self.course % 360

        # 更新历史记录（保留最近100个数据点）
        self.speed_history.pop(0)
        self.speed_history.append(self.speed)
        self.course_history.pop(0)
        self.course_history.append(self.course)

    def update_gps_position(self):
        """根据公交车速度和航向更新GPS位置（更精细的位置计算）"""
        import math

        # 10ms内移动的距离（米）
        distance = self.speed * 1000 / 3600 * 0.01

        # 航向角转弧度
        bearing = math.radians(self.course)
        lat1 = math.radians(self.latitude)
        lon1 = math.radians(self.longitude)

        # 高精度大圆航行计算
        lat2 = math.asin(
            math.sin(lat1) * math.cos(distance / EARTH_RADIUS) +
            math.cos(lat1) * math.sin(distance / EARTH_RADIUS) * math.cos(bearing)
        )
        lon2 = lon1 + math.atan2(
            math.sin(bearing) * math.sin(distance / EARTH_RADIUS) * math.cos(lat1),
            math.cos(distance / EARTH_RADIUS) - math.sin(lat1) * math.sin(lat2)
        )

        # 转回角度并添加GPS噪声（10ms精度优化）
        # 激烈驾驶模式下GPS噪声更大
        noise_factor = 1.5 if self.aggressive else 1.0
        self.latitude = math.degrees(lat2) + np.random.normal(0, 0.000005 * noise_factor)
        self.longitude = math.degrees(lon2) + np.random.normal(0, 0.000005 * noise_factor)
        self.altitude += np.random.normal(0, 0.05 * noise_factor)

        # 站点附近速度修正
        if self.state in ["STOP", "BRAKING", "SLOWING"]:
            self.speed = min(self.speed, 10 if self.aggressive else 5)  # 激烈驾驶进站速度更快

    def update_sensor_data(self):
        """更新传感器数据（10ms采样频率适配）"""
        import math

        # 状态机更新
        self.update_state()

        # 物理参数更新
        self.update_vehicle_physics()

        # GPS位置更新
        self.update_gps_position()

        # 加速度计算（更激烈的驾驶特性）
        accel_x = self.acceleration / 9.81  # 纵向加速度（前进/后退）
        accel_z = 1.0  # 重力分量

        # 转弯横向加速度（更激进的转向）
        if abs(self.angular_velocity) > 0.5:
            angular_velocity_rad = math.radians(abs(self.angular_velocity))
            turning_radius = (self.speed * 1000 / 3600) / angular_velocity_rad
            centripetal_accel = (self.speed * 1000 / 3600) ** 2 / turning_radius
            accel_y = centripetal_accel / 9.81

            # 转向方向修正（左转为负）
            if self.angular_velocity > 0:
                accel_y = -accel_y
        else:
            accel_y = 0.0

        # 添加传感器噪声（更激烈的驾驶会产生更大的噪声）
        noise_scale = 1.5 if self.aggressive else 1.0
        accel_x += np.random.normal(0, accel_noise * math.sqrt(100) * noise_scale)
        accel_y += np.random.normal(0, accel_noise * math.sqrt(100) * noise_scale)
        accel_z += np.random.normal(0, accel_noise * math.sqrt(100) * noise_scale)

        # 陀螺仪数据（更激进的转向）
        gyro_z = self.angular_velocity
        gyro_z += np.random.normal(0, gyro_noise * math.sqrt(100) * noise_scale)
        gyro_z = max(min(gyro_z, self.gyro_range), -self.gyro_range)

        # x/y轴陀螺仪噪声
        self.gyro_x = np.random.normal(0, gyro_noise * math.sqrt(100) * 0.5 * noise_scale)
        self.gyro_y = np.random.normal(0, gyro_noise * math.sqrt(100) * 0.5 * noise_scale)

        # 量程限制
        self.accel_x = max(min(accel_x, self.accel_range), -self.accel_range)
        self.accel_y = max(min(accel_y, self.accel_range), -self.accel_range)
        self.accel_z = max(min(accel_z, self.accel_range), -self.accel_range)

        # GPS数据微调
        self.altitude += np.random.normal(0, 0.1)
        self.speed = max(0, self.speed + np.random.normal(0, 0.1))
        self.course = (self.course + np.random.normal(0, 0.2)) % 360

        # GPS信号质量
        if self.state == "STOP":
            self.fix_quality = 2
        else:
            # 激烈驾驶模式下GPS信号质量波动更大
            self.fix_quality = \
            random.choices([1, 2], weights=[0.3 if self.aggressive else 0.2, 0.7 if self.aggressive else 0.8], k=1)[0]
        self.satellites_visible = random.randint(6, 12) if self.fix_quality > 0 else 0

        # 更新时间戳（精确到毫秒）
        self.timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]

        # 保存历史数据
        self.last_accel_x = self.accel_x
        self.last_accel_y = self.accel_y
        self.last_accel_z = self.accel_z
        self.last_gyro_x = self.gyro_x
        self.last_gyro_y = self.gyro_y
        self.last_gyro_z = self.gyro_z

        # 打印关键信息（每100ms输出一次）
        '''if int(self.time_in_state * 100) % 10 == 0:
            mode = "激烈驾驶" if self.aggressive else "正常驾驶"
            print(
                f"[10ms采样] 公交车 {self.device_id} - 驾驶模式: {mode}, 状态: {self.state}, 速度: {self.speed:.1f}km/h")
            print(f"  加速度: ({self.accel_x:.4f}, {self.accel_y:.4f}, {self.accel_z:.4f}) g")
            print(f"  角速度: ({self.gyro_x:.4f}, {self.gyro_y:.4f}, {self.gyro_z:.4f}) dps")
            print(f"  GPS: ({self.latitude:.6f}, {self.longitude:.6f}), 站点: {self.current_station}")'''



